#include "TlmMgr.h"
#include "DSP2833x_Device.h"
#include "UARTDriverA.h"
#include "UARTDriverB.h"
#include "DataFile.h"
#include <string.h>
#include <stdio.h>

extern UARTDriverA uartDriverA;
extern UARTDriverB uartDriverB;
extern SDataFile df;

void TlmMgr::Init()
{
	m_PhaseCounter = 0;
}

void TlmMgr::Process()
{
	switch( m_PhaseCounter )
	{
		case 0:
			SendMissionData();			
			SendRPY();
			break;
		case 1:
			SendGPS();
			SendAccRot();
			break;
		case 2:
			SendPressures();
			SendRPY();
			break;
		case 3:
			SendTemperatures();
			SendAccRot();
			break;
		case 4:
			SendVoltages();
			SendRPY();
			break;
		case 5:
			SendFiltered();
			SendAccRot();
			break;
		case 6:
			SendRPY();
			break;
		case 7:
			SendAccRot();
			break;
		case 8:
			SendRPY();
			break;
		case 9:
			SendAccRot();
			break;
	}
	
	if( ++m_PhaseCounter >= 10) m_PhaseCounter = 0;
	
	// send real data
	//char tmp[200];
	//sprintf(tmp, "$TELE,%d,%s,%0.6Lf,%0.6Lf,%0.2f,%d,%0.1f,%0.1f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f#\r\n", 
	//	controller.GetState(), df.SatTime, df.Longitude, df.Latitude, df.Altitude, df.SatNum, 
	//	df.Pressure1*1000, df.Pressure2*1000, df.TemperatureADC1, df.TemperatureADC2, df.TemperatureExternal,
	//	df.Levels[0],df.Levels[1], df.Levels[2], df.Rots[0],df.Rots[1], df.Rots[2]);  
	//uartDriverB.SendData(tmp);	
	//uartDriverA.SendData(tmp); // DEBUG
}

void TlmMgr::SendRPY()
{
	sprintf(m_Buffer, "$RPY,%0.2f,%0.2f,%0.2f#\r\n", df.RPY[0]*180/3.14159, df.RPY[1]*180/3.14159, df.RPY[2]*180/3.14159);
	QueueData();
}

void TlmMgr::SendGPS()
{
	sprintf(m_Buffer, "$GPS,%s,%0.6Lf,%0.6Lf,%0.2f,%d#\r\n", df.SatTime, df.Longitude, df.Latitude, df.Altitude, df.SatNum);
	QueueData();
}

void TlmMgr::SendPressures()
{
	sprintf(m_Buffer, "$PRESS,%0.1f,%0.1f,%0.3f#\r\n", df.Pressure1*1000, df.Pressure2*1000, df.PressureAlt);
	QueueData();
}

void TlmMgr::SendTemperatures()
{
	sprintf(m_Buffer, "$TMP,%0.3f,%0.3f,%0.3f#\r\n", df.TemperatureADC1, df.TemperatureADC2, df.TemperatureExternal);
	QueueData();
}

void TlmMgr::SendVoltages()
{
	sprintf(m_Buffer, "$VOLT,%0.3f,%0.3f#\r\n", df.Power5VADC1, df.Power5VADC2);
	QueueData();
}

void TlmMgr::SendFiltered()
{
	sprintf(m_Buffer, "$FILT,%0.3f,%0.3f#\r\n", df.AltFiltered, df.VertSpeed);
	QueueData();
}

void TlmMgr::SendAccRot()
{
	sprintf(m_Buffer, "$ROT,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f#\r\n", df.Levels[0], df.Levels[1], df.Levels[2], df.Rots[0], df.Rots[1], df.Rots[2]);
	QueueData();
}

void TlmMgr::SendMissionData()
{
	sprintf(m_Buffer, "$MIS,%d,%0.3Lf,%0.4f,%d#\r\n", df.State, df.MissionTime, df.LoopTime, df.ComTXFails);
	QueueData();
}

void TlmMgr::QueueData()
{
	bool ok = uartDriverA.QueueData(m_Buffer);	
	if( ok != true ) df.ComTXFails++;
}
